#pragma once
#include "StdAfx.h"
#include "ChmDevice.h"
#include <valarray> // for sqrt function

#pragma unmanaged

unsigned	axis	= 0;
unsigned	idx		= 0;
double		sum		= 0;

ChmDevice::ChmDevice(wstring deviceName)
{
	/// <summary>
	/// hardware settings
	/// </summary>
	this->deviceName	= deviceName;
	ipAddress			= L"10.30.203.48";
	port				= 4321;
	hm					= NULL;

	/// <summary>
	/// haptic environment settings
	/// </summary>
	currentState		= FCSSTATE_UNKNOWN;
	mass				= 5.0;
	dampingRatio		= 1.0;
	for (unsigned row = X; row<=Z; row++) {
		for (unsigned col = X; col<=Z; col++) {
			if (row==col)
				envK[row][row] = 50000.0;
			else
				envK[row][col] = 0.0;
			envB[row][col] = 2*sqrt( 2*dampingRatio*mass*envK[row][col] );
		}
	}
	biasForce			= vector<double>(NAXES, 0.0);

	/// <summary>
	/// workspace limits
	/// </summary>
	pSphere				= NULL;
	pBlock				= NULL;

	/// <summary>
	/// data collection settings
	/// </summary>
	armedFlag			= false;
	collectFlag			= false;
	duration			= 1.0; // s
	pretrigger			= 0.7; // s
	dataVec				= vector<double>(NSENSORST, 0.0);
	dataMat				= vector<vector<double>>(ceil(duration*MAXFREQ), vector<double>(NSENSORST));
	dataPreTrig			= vector<vector<double>>(ceil(pretrigger*MAXFREQ), vector<double>(NSENSORST));
	cbCount				= 0;
	//dataMat				= vector<vector<double>>((unsigned) ceil(NSENSORST*duration*MAXFREQ), vector<double>(NSENSORST, 0.0));
	dataBuf				= new FcsInfoBuffer;
	fileName			= L".\\data.mat";
	pmat				= NULL;

	QueryPerformanceFrequency( &tickFreq ); // find the frequency of the tick counts on the local computer
	QueryPerformanceCounter( &tCurrent );	// initialize current ticks
	curT				= 0.0;	// current time
	prevT				= 0.0;
	startT				= 0.0;	// start time
	curPos				= vector<double>(NAXES, 0.0);
	curAcc				= vector<double>(NAXES, 0.0);
	curVel				= vector<double>(NAXES, 0.0);
	curFor				= vector<double>(NAXES, 0.0);
	curGim				= vector<double>(NAXES, 0.0);

	zeroPos				= vector<double>(NAXES, 0.0);
	zeroGim				= vector<double>(NAXES, 0.0);

	tarPos				= vector<double>(NAXES, 0.0);
	tarGim				= vector<double>(NAXES, 0.0);
	tarFor				= vector<double>(NAXES, 0.0);
	tarFTol				= 1.0;
	tarGimbalTol		= 5;

	tarHoldT			= 0.7;
	tarHeldT			= 0.0;
	tarHeldFlag			= false;

	pertInfo			= new CPerturbation;
	pertInfo->maxT		= 0.0;
	
	pertEnableFlag		= false;
	pertOnFlag			= false;
	pertT				= 0.0;
	pertZero			= vector<double>(NAXES, 0.0);
	pertPos				= vector<double>(NAXES, 0.0);
	pertVel				= vector<double>(NAXES, 0.0);
	pertK				= vector<vector<double>>(3, vector<double>(3,0.0));
	pertB				= vector<vector<double>>(3, vector<double>(3,0.0));
	
	//pertDelay			= 0.0;
}
ChmDevice::~ChmDevice(void)
{
	Disconnect();
	delete hm;
	delete dataBuf;
}

bool ChmDevice::Connect(void)
{
#if !_DEBUG
	// convert wstring to string for calling the appropriate hm Device
	size_t tmp;	// this will hold the number of characters converted. (not used for anything)
	char* devName = (char*) malloc(255);
	wcstombs_s(&tmp, devName, 255, deviceName.c_str(), 255);
	// try connecting to the specified hapticMASTER
	hm	= ConnectToHapticMASTER(devName);
	Sleep(1000);
	if (hm == NULL) // if the hapticMASTER is not connected
		return false;

	// try initializing the hapticMASTER
	GetState();
	if ( (currentState != FCSSTATE_NORMAL) && (currentState != FCSSTATE_FIXED) && (currentState != FCSSTATE_FREE) ) {
			 hm->SetRequestedState(FCSSTATE_INITIALISING);
			 while ( currentState != FCSSTATE_INITIALISED ) {
				 Sleep(1000);
				 GetState();
			 }
	}
	// add connection checking here


	hm->DeleteAll(); // delete all haptic objects
	SetMass(mass);   // set mass

	/// Remove all existing info blocks and add only the ones below.
	/// This sets up the order in which the data is collected.
	hm->RemoveInfoBlock(FCSPRM_ALL);		// remove all data collection
	hm->AddInfoBlock(FCSPRM_POSITION);		// add position
	hm->AddInfoBlock(FCSPRM_VELOCITY);		// add velocity
	hm->AddInfoBlock(FCSPRM_ACCELERATION);	// add acceleration
	hm->AddInfoBlock(FCSPRM_FORCE);			// add force
	hm->AddInfoBlock(FCSPRM_POTMETERS);		// add gimbal
	

	// free the space used by the temporary char* used for connecting
	if (devName)
		delete(devName);

	SetState(FCSSTATE_FIXED);

	// setup scope
	if (hm->SupportScope() != SCOPE_NOT_SUPPORTED)	// scope is supported
	{
		// Pass a message to the GUI that the scope is supported

		// Setup the IP address and port for the scope
		ScopeSetIpAddress("10.4.0.34"); // Set the Scope IP address (eventually read this in)
		ScopeSetPortNr(SCOPE_DEFAULT_PORTNR);

		// add appropriate channels to the scope
		hm->RemoveAllScopeChannels();				// remove all scope channels
		hm->AddScopeChannel(FCSPRM_POSITION);		// add position
		hm->AddScopeChannel(FCSPRM_VELOCITY);		// add velocity
		hm->AddScopeChannel(FCSPRM_ACCELERATION);	// add acceleration
		hm->AddScopeChannel(FCSPRM_FORCE);			// add force
		hm->AddScopeChannel(FCSPRM_POTMETERS);		// add gimbal


		// setup other scope parameters
		if (ScopeSetSampleRate(SCOPE_RATE_2500HZ) != SCOPE_NO_ERROR)		// set sampling rate to 2500 Hz
		{
			// send error
		}
		if (ScopeSetSyncRate(SCOPE_RATE_2500HZ) != SCOPE_NO_ERROR)			// set the sync rate to 2500 Hz
		{
			// send error
		}
	}
	else	// if scope is not supported
	{
		// Sometime in the future,
		// pass a message to the GUI that the scope is not supported
		return false;
	}

	// set clock pulse to low
	hm->ParallelPortWriteBit(FCSPRM_PARALLELPORT_CONTROL, CLCK_BIT, CLCK_LO);
	// set trigger pulse to low
	hm->ParallelPortWriteBit(FCSPRM_PARALLELPORT_CONTROL, TRIG_BIT, TRIG_LO);
#endif

	return true;
}
bool ChmDevice::Disconnect() {
	if (hm == NULL)
		return true;
			
	hm->GetCurrentState(currentState);
	if ( (currentState != FCSSTATE_OFF) && (currentState != FCSSTATE_FAIL) ) {
		hm->SetRequestedState(FCSSTATE_OFF);

		while (currentState != FCSSTATE_OFF) {
			hm->GetCurrentState(currentState);
			Sleep(100);
		}
	}
	hm->DeleteAll(); // delete all haptic objects


	/// <summary>
	/// Remove all existing info blocks.
	/// </summary>
	hm->RemoveInfoBlock(FCSPRM_ALL);

	// close the scope
	ScopeClose();
	
	return true;
}

bool ChmDevice::SetState(FCSSTATE state) {
	unsigned timeOut = 0;

	// check if hapticMASTER exists
	if (hm==NULL)
		return false;

	// check if current state and the requested state are the same
	GetState();
	if (currentState == state)
		return true;
	
	// if the requested state is not FCSSTATE_INITIALISED,
	// set it it FCSSTATE_INITIALISED first
	if (state != FCSSTATE_INITIALISED) {
		if ( !SetState(FCSSTATE_INITIALISED) )
			return false;
	}

	// request to set the state
	if ( hm->SetRequestedState(state) ) {
		// if setting the state failed
		return false;
	} else {
		// if the state was successfully requested
		// ensure that the state was set
		while (currentState!=state && timeOut<10) {
			GetState();
			Sleep(100);
			timeOut++;
		}
	}

	return (currentState == state) ? true : false;
}
FCSSTATE ChmDevice::GetState(void) {
	// check if hapticMASTER exists
	if (hm==NULL)
		currentState = FCSSTATE_UNKNOWN;
	else
		hm->GetCurrentState(currentState);

	return currentState;
}

bool ChmDevice::SetMass(double mass) {
	if (hm==NULL)
		return false;

	this->mass = mass;
	return (!hm->SetParameter(FCSPRM_INERTIA, mass)); 
}
void ChmDevice::SetEnvironment(double stiffness[3][3], double damping[3][3]) {
	for (unsigned row=X; row<=Z; row++) {
		for (unsigned col=X; col<=Z; col++) {
			envK[row][col] = stiffness[row][col];
			envB[row][col] = damping[row][col];
		}
	}
}
void ChmDevice::SetEnvironment(double stiffness[3][3], double zeta) {
	dampingRatio = zeta;
	for (unsigned row=X; row<=Z; row++) {
		for (unsigned col=X; col<=Z; col++) {
			envK[row][col] = stiffness[row][col];
			envB[row][col] = 2*dampingRatio*sqrt( 2*mass*envK[row][col] );
		}
	}
}

void ChmDevice::SetBiasForce(vector<double> f)
{
	for (axis = X; axis<NAXES; axis++) 
		biasForce[axis] = f[axis];
}
void ChmDevice::SetTargetForce(vector<double> f, double tol, double hold)
{
	for (axis = X; axis<NAXES; axis++)
		tarFor[axis] = f[axis];

	tarFTol = tol;
	tarHoldT = hold;
}
void ChmDevice::SetTargetPosition(vector<double> p, double tol, double time=0.0)
{
	for (axis = X; axis<NAXES; axis++)
		tarPos[axis] = p[axis];
}
void ChmDevice::SetTargetGimbal(vector<double> g, double tol)
{
	for (axis = X; axis<NAXES; axis++)
		tarPos[axis] = g[axis];
}


void ChmDevice::CreateSphericalLimits(double radius, double center[3]) {
	if (hm!=NULL)
		pSphere = hm->CreateSphere(center, radius, 50000.0, 500.0, 1.0, 1.2, 0.5, 0.05);
}
void ChmDevice::CreateCuboidalLimits(double lengths[3], double center[3]) {
	double orientation[] = {0.0, 0.0, 0.0};
	if (hm!=NULL) 
		pBlock = hm->CreateBlock(center, orientation, lengths, 50000, 500, 1.0, 1.5, 0.5, 0.05);
}

vector<double> ChmDevice::GetPosition(void)
{
	double pos[3];
#if _DEBUG
	for (axis=X; axis<NAXES; axis++) {
		this->curPos[axis] = 0.0;
	}
#else
	hm->GetParameter(FCSPRM_POSITION, pos);
	for (axis=X; axis<NAXES; axis++)
		this->curPos[axis] = pos[axis];
#endif
	return this->curPos;
}
vector<double> ChmDevice::GetVelocity(void)
{
	double vel[3];
#if _DEBUG
	for (axis=X; axis<NAXES; axis++)
		curVel[axis] = 0.0;
#else
	hm->GetParameter(FCSPRM_POSITION, vel);
	for (axis=X; axis<NAXES; axis++)
		curVel[axis] = vel[axis];
#endif
	return curVel;
}
vector<double> ChmDevice::GetForce(void)
{
	double force[3];
#if _DEBUG
	for (axis=X; axis<NAXES; axis++)
		curFor[axis] = 0.0;
#else
	hm->GetParameter(FCSPRM_FORCE, force);
	for (axis=X; axis<NAXES; axis++)
		curFor[axis] = force[axis];
#endif
	return curFor;
}
vector<double> ChmDevice::GetGimbal(void)
{
	double gimbal[3];
#if _DEBUG
	for (axis=X; axis<NAXES; axis++)
		curGim[axis] = 0.0;
#else
	hm->GetParameter(FCSPRM_POTMETERS, gimbal);
	for (axis=X; axis<NAXES; axis++)
		curGim[axis] = gimbal[axis];
#endif
	return curGim;
}

vector<double> ChmDevice::FetchPosition(void)
{
	return curPos;
}
vector<double> ChmDevice::FetchForce(void)
{
	return curFor;
}
vector<double> ChmDevice::FetchGimbal(void)
{
	return curGim;
}
vector<double> ChmDevice::FetchZeroPosition(void)
{
	return zeroPos;
}
vector<double> ChmDevice::FetchZeroGimbal(void)
{
	return zeroGim;
}

bool ChmDevice::ZeroForce(void)
{
	if (hm==NULL)
		return false;

	hm->CalibrateForcesensor();
	return true;
}
vector<double> ChmDevice::ZeroPosition(void)
{
	zeroPos = vector<double>(NAXES, 0.0);

#if !_DEBUG
	for (unsigned sample = 0; sample<NCALIBPTS; sample++) {
		GetPosition();
		for (axis = X; axis<NAXES; axis++) {
			zeroPos[axis] += (curPos[axis]/NCALIBPTS);
		}
	}
#endif
	return zeroPos;
}
vector<double> ChmDevice::ZeroGimbal(void)
{
	zeroGim = vector<double>(NAXES, 0.0);
#if !_DEBUG
	for (unsigned sample = 0; sample<NCALIBPTS; sample++) {
		GetGimbal();
		for (axis = X; axis<NAXES; axis++) {
			zeroGim[axis] += curGim[axis]/NCALIBPTS;
		}
	}
#endif
	return zeroGim;
}

bool ChmDevice::ArmCollection(wstring fName, double dur, double pretrig)
{
	/// <summary>
	/// needs implementation of the following steps:
	/// 1. verify hm exists
	/// 2. set the filename, duration and pretrigger time
	/// 3. open the matlab file
	/// </summary>
	fileName = fName;
	duration = dur;
	pretrigger = pretrig;
	collectFlag = false;
	tarHeldFlag = false;
	armedFlag	= false;

	vector<double> temp(NSENSORST, 0.0);
	
	// allocate memory
	dataMat.clear();
	dataMat.reserve((unsigned) ceil(duration*MAXFREQ));

	dataPreTrig.clear();
	dataPreTrig.reserve((unsigned) ceil(pretrigger*MAXFREQ));
	for (unsigned i = 0; i<dataPreTrig.capacity(); i++)
		dataPreTrig.push_back(temp);
	cbCount = 0;
	armedFlag = true;
	
	// check if pMat is already open
	if (pmat != NULL)
		matlab::matClose(pmat);
	pmat = NULL;

	// open mat file
	char* matfName = new char[255];
	sprintf(matfName,"%ls", fileName.c_str());
	pmat = matlab::matOpen(matfName, "w");
	if (pmat == NULL) {
		//errStr = L"File was not created.";
		//return errStr;
		return false;
	}

	// open scope file for storing the data
	char* scopefName = new char[255];
	sprintf(scopefName, "%ls", fileName.c_str());
	if (ScopeSetDataFileName(scopefName) != SCOPE_NO_ERROR)	// set filename for data collection
	{
		// send error
	}
	if (ScopeSetFixedSampleCount(ceil(duration * 2500)) != SCOPE_NO_ERROR) // set the number of sample points
	{
		// send error
	}

	return true;
}
bool ChmDevice::EnableDataCollection(void) {
	collectFlag = true;
	startT = pertT = curT;
	tarHeldT = 0.0;
	// set trigger pulse high
	//hm->ParallelPortWriteBit(FCSPRM_PARALLELPORT_CONTROL, TRIG_BIT, TRIG_HI);
	return false;
}
bool ChmDevice::EnableDataCollection(bool flag) {
	startT = pertT = curT;
	tarHeldT = 0.0;
	pertEnableFlag = flag;
	pertOnFlag=false;
	collectFlag = true;
	// set trigger pulse high
	//hm->ParallelPortWriteBit(FCSPRM_PARALLELPORT_CONTROL, TRIG_BIT, TRIG_HI);
	return false;
}
wstring ChmDevice::SaveData()
{
	double *tempPtr;
	int status;
	unsigned i, j;

	matlab::mxArray *TrigTime;
	TrigTime = matlab::mxCreateDoubleMatrix(1, 1, matlab::mxREAL);
	tempPtr = matlab::mxGetPr(TrigTime);
	tempPtr[0] = startT;
	status = matlab::matPutVariable(pmat, "TrigTime", TrigTime);
	if (status != 0) {
		errStr = L"Could not copy variable into matlab array.";
		return errStr;
	} 
	matlab::mxDestroyArray(TrigTime);

	matlab::mxArray *PertOnset;
	PertOnset = matlab::mxCreateDoubleMatrix(1, 1, matlab::mxREAL);
	tempPtr = matlab::mxGetPr(PertOnset);
	tempPtr[0] = pertT;
	status = matlab::matPutVariable(pmat, "PertOnset", PertOnset);
	if (status != 0) {
		errStr = L"Could not copy variable into matlab array.";
		return errStr;
	}
	matlab::mxDestroyArray(PertOnset);
	
	// create matlab array to store data and allocate memory
	matlab::mxArray *sensorData;
	sensorData = matlab::mxCreateDoubleMatrix( dataPreTrig.size(), NSENSORST, matlab::mxREAL );
	if (sensorData == NULL) {
		errStr = L"Could not create matlab matrix.";
		return errStr;
	}
	// copy data to matlab array -- memcpy doesn't work
	tempPtr = matlab::mxGetPr(sensorData);
	for (i = 0; i<dataPreTrig.size(); i++) // time stamp
		for (j = 0; j<NSENSORST; j++) // sensor
			tempPtr[j*dataPreTrig.size() + i] = dataPreTrig[i][j]; // copy pre-trigger data
	status = matlab::matPutVariable(pmat, "pretrig_data", sensorData);
	if (status != 0) {
		errStr = L"Could not copy variable into matlab array.";
		return errStr;
	} 
	matlab::mxDestroyArray(sensorData);
	
	sensorData = matlab::mxCreateDoubleMatrix( dataMat.size(), NSENSORST, matlab::mxREAL );
	if (sensorData == NULL) {
		errStr = L"Could not create matlab matrix.";
		return errStr;
	}
	tempPtr = matlab::mxGetPr(sensorData);
	for (i = 0; i<dataMat.size(); i++) // time stamp
		for (j = 0; j<NSENSORST; j++) // sensor
			tempPtr[j*dataMat.size() + i] = dataMat[i][j]; // copy post-trigger data
			//tempPtr[j*dataMat.size()/NSENSORST + i] = dataMat[i*NSENSORST + j]; // note transpose when going from C++ to MATLAB
	
	// store it into the file as variable "hmdata"
	status = matlab::matPutVariable(pmat, "posttrig_data", sensorData);
	if (status != 0) {
		errStr = L"Could not copy variable into matlab array.";
		return errStr;
	} 

	// free up the memory space (doesn't remove the variable itself)
	matlab::mxDestroyArray(sensorData);

		
	if (matlab::matClose(pmat) != 0) {
		errStr = L"Error in closing matlab file.";
		return errStr; 
	}
	else {
		pmat = NULL;
	}
	errStr = L"Success.";
	return errStr;
}

bool ChmDevice::LoadPerturbation(wstring pertFile) 
{
	using namespace matlab;
	// open the mat file
	char* matfName = new char[255];
	sprintf(matfName,"%ls", pertFile.c_str());
	matlab::MATFile	 *pertmat;
	pertmat = matlab::matOpen(matfName, "r");
	if (pertmat == NULL) {
		//errStr = L"File was not created.";
		//return errStr;
		return false;
	}

	matlab::mxArray *position, *velocity, *stiffness, *freq, *delay;
	double *pos, *vel, *stiff;
	
	position = matGetVariable(pertmat, "position");
	if (position == NULL)
		return false;
	velocity = matGetVariable(pertmat, "velocity");
	if (velocity == NULL)
		return false;
	stiffness = matGetVariable(pertmat, "stiffness");
	if (stiffness == NULL)
		return false;
	freq = matGetVariable(pertmat, "freq");
	if (freq == NULL)
		return false;
	delay = matGetVariable(pertmat, "delay");
	if (delay == NULL)
		return false;

	if (mxGetNumberOfDimensions(position) != 2)
		return false;
	if (mxGetNumberOfDimensions(velocity) != 2)
		return false;
	if (mxGetNumberOfDimensions(stiffness) != 2)
		return false;

	const mwSize *dims;
	mwSize row, col;
	
	pertInfo->freq = *(double*) mxGetData(freq);
	pertInfo->delay = *(double*) mxGetData(delay);

	pertInfo->timePeriod = 1/pertInfo->freq;

	pos = (double*) mxGetData(position);
	vel = (double*) mxGetData(velocity);
	stiff = (double*) mxGetData(stiffness);

	dims = mxGetDimensions(position);
	if (dims[1] != 3)
		return false;
	dims = mxGetDimensions(velocity);
	if (dims[1] != 3)
		return false;
	dims = mxGetDimensions(stiffness);
	if (dims[1] != 9)
		return false;

	pertInfo->nSamples = dims[0];
	pertInfo->maxT = dims[0] * pertInfo->timePeriod;

	vector<double> tmp(3, 0.0);
	vector<double> tmp2(3, 0.0);

	pertInfo->position.clear();		pertInfo->position.reserve(dims[0]);
	pertInfo->velocity.clear();		pertInfo->velocity.reserve(dims[0]);
	for (row = 0; row< dims[0]; row++) {
		for (col=0; col<3; col++) {
			tmp[col] = pos[row + col*dims[0]];
			tmp2[col] = vel[row + col*dims[0]];
		}
		pertInfo->position.push_back(tmp);
		pertInfo->velocity.push_back(tmp);
	}

	tmp.resize(9,0.0);
	tmp2.resize(9,0.0);
	pertInfo->stiffness.clear();	pertInfo->stiffness.reserve(dims[0]);
	pertInfo->damping.clear();		pertInfo->damping.reserve(dims[0]);
	for (row=0; row<dims[0]; row++) {
		for (col=0; col<9; col++) {
			tmp[col] = stiff[row + col*dims[0]];
			tmp2[col] = 2*sqrt(abs(tmp[col] * mass));
		}
		pertInfo->stiffness.push_back(tmp);
		pertInfo->damping.push_back(tmp2);
	}

	return true;
}


//wstring ChmDevice::SaveData(unsigned count) 
//{
//	unsigned startCount = count;
//	// create matlab array to and allocate memory
//	matlab::mxArray *sensorData;
//	sensorData = matlab::mxCreateDoubleMatrix( dataMat.size() + dataPreTrig.size(), NSENSORST, matlab::mxREAL );
//	if (sensorData == NULL) {
//		errStr = L"Could not create matlab matrix.";
//		return errStr;
//	}
//
//	// copy data to matlab array -- memcpy doesn't work
//	double *tempPtr;
//	tempPtr = matlab::mxGetPr(sensorData);
//	for (unsigned i = 0; i<dataMat.size(); i++) // time stamp
//		for (unsigned j = 0; j<NSENSORST; j++) // sensor
//			tempPtr[j*dataMat.size() + i] = dataPreTrig[i][j];
//	
//
//	for (unsigned i = 0; i<dataMat.size(); i++) // time stamp
//		for (unsigned j = 0; j<NSENSORST; j++) // sensor
//			tempPtr[j*dataMat.size() + i] = dataMat[i][j];
//			//tempPtr[j*dataMat.size()/NSENSORST + i] = dataMat[i*NSENSORST + j]; // note transpose when going from C++ to MATLAB
//	
//	// store it into the file as variable "hmdata"
//	int status;
//	status = matlab::matPutVariable(pmat, "hmdata", sensorData);
//	if (status != 0) {
//		errStr = L"Could not copy variable into matlab array.";
//		return errStr;
//	} 
//
//	// free up the memory space (doesn't remove the variable itself)
//	matlab::mxDestroyArray(sensorData);
//		
//	if (matlab::matClose(pmat) != 0) {
//		errStr = L"Error in closing matlab file.";
//		return errStr; 
//	}
//	else {
//		pmat = NULL;
//	}
//	errStr = L"Success.";
//	return errStr;
//}
